#ifndef _REBO_REPLAN_FSM_H_
#define _REBO_REPLAN_FSM_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <mavros_msgs/RCIn.h>
#include <sensor_msgs/NavSatFix.h>
#include <mavros_msgs/State.h>
#include<mavros_msgs/WaypointList.h>

#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>
#include <vector>
#include <visualization_msgs/Marker.h>

#include <bspline_opt/bspline_optimizer.h>
#include <plan_env/grid_map.h>
#include <ego_planner/Bspline.h>
#include <ego_planner/DataDisp.h>
#include <plan_manage/planner_manager.h>
#include <traj_utils/planning_visualization.h>
#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>
using std::vector;

namespace ego_planner
{

  class EGOReplanFSM
  {

  private:
    /* ---------- flag ---------- */
    enum FSM_EXEC_STATE
    {
      INIT,
      WAIT_TARGET,
      GEN_NEW_TRAJ,
      REPLAN_TRAJ,
      EXEC_TRAJ,
      EMERGENCY_STOP
    };
    enum TARGET_TYPE
    {
      MANUAL_TARGET = 1,
      PRESET_TARGET = 2,
      REFENCE_PATH = 3
    };

    /* planning utils */
    EGOPlannerManager::Ptr planner_manager_;
    PlanningVisualization::Ptr visualization_;
    ego_planner::DataDisp data_disp_;

    /* parameters */
    int target_type_; // 1 mannual select, 2 hard code
    double no_replan_thresh_, replan_thresh_;
    double waypoints_[50][3];
    int waypoint_num_;
    double planning_horizen_, planning_horizen_time_;
    double emergency_time_;
    bool neednewplan_{false};


    //by zhou define variables
    double channel1_value, channel2_value, channel3_value, channel4_value, channel7_value, channel8_value;
    double max_vel_fromrc{1.0};
    bool global_global_flag{false};
    bool global_local_flag{false};
    bool mission_waypoint_flag{false};
    bool first_to_plan_withwps{false};
    bool wait_target_flag{false};
    bool wait_nav_flag{false};
    bool exploration_flag{false};
    bool needtochangepriciple{false};
    bool need_init_flag{true};
    int wait_target_cnt{0};
    int nav_target_cnt{0};
    ros::Time last_nav_time{0.0};
    ros::Time last_target_time{0.0};
    int frontier_x_min,frontier_y_min,frontier_x_max,frontier_y_max;
    int min_x_cord, min_y_cord;
    int id_cnt{0};
    double x_target{0.0};
    double y_target{0.0};
    double x_target_last{0.0};
    double y_target_last{0.0};
    double explore_dist{3.0};
    double iter_step{0.1};

    struct Box
    {
      float x_min;
      float y_min;
      float z_min;
      float x_max;
      float y_max;
      float z_max;
      int id;
    };

    int clusterId{0};

    double dist_height{1.0};
    nav_msgs::Odometry global_local_pose;
    sensor_msgs::NavSatFix gps_llh;
    
    mavros_msgs::State px4_state;

    Eigen::MatrixXd frontier_matrix;


    /* planning data */
    bool trigger_, have_target_, have_odom_, have_new_target_;
    FSM_EXEC_STATE exec_state_;
    int continously_called_times_{0};

    Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state
    Eigen::Quaterniond odom_orient_;

    Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state
    Eigen::Vector3d end_pt_, end_vel_;                                       // goal state
    Eigen::Vector3d local_target_pt_, local_target_vel_;                     // local target state
    int current_wp_;

    bool flag_escape_emergency_;

    /* ROS utils */
    ros::NodeHandle node_;
    ros::Timer exec_timer_, safety_timer_, global_timer_, global_mission_timer_, rc_control_timer_, frontier_timer_, navigation_timer_;


    ros::Subscriber waypoint_sub_, odom_sub_, px4_state_sub_, rcin_sub_,mission_waypoint_sub_, global_local_pose_sub_,global_global_pose_sub_;
    ros::Subscriber max_vel_sub_, new_plan_sub_, exploration_sub_;

    ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, goal_pub_;
    ros::Publisher frontier_point_pub_, exploration_finished_pub_, marker_all_pub_, marker_id_pub_;

    /* helper functions */
    bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method
    bool callEmergencyStop(Eigen::Vector3d stop_pos);                          // front-end and back-end method
    bool planFromCurrentTraj();


    /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */
    void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
    std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> timesOfConsecutiveStateCalls();
    void printFSMExecState();

    void planGlobalTrajbyGivenWps();
    void planGlobalTrajbyGivenMission(mavros_msgs::WaypointList mission_waypoints);
    
    void getLocalTarget();

    /* ROS functions */
    void execFSMCallback(const ros::TimerEvent &e);
    void checkCollisionCallback(const ros::TimerEvent &e);
    void globalplanCallback(const ros::TimerEvent &e);
    void frontierTimerCallback(const ros::TimerEvent &e);
    void navigationCallback(const ros::TimerEvent &e);
  
    void rc_control_callback(const ros::TimerEvent &e);
    
    void waypointCallback(const nav_msgs::PathConstPtr &msg);
    void odometryCallback(const nav_msgs::OdometryConstPtr &msg);
    void newplanCallback(const std_msgs::Bool &msg);
    void explorationCallback(const std_msgs::Float32MultiArray &msg);
    void maxvelCallback(const std_msgs::Float32MultiArray &msg);
    void rcin_callback(const mavros_msgs::RCIn & rcvalue);
    void state_callback(const mavros_msgs::State::ConstPtr& msg);
    void mission_point_callback(const mavros_msgs::WaypointList &msg);
    void global_local_pose_callback(const nav_msgs::Odometry &msg);
    void global_global_pose_callback(const sensor_msgs::NavSatFix &msg);
    void human_callback(const swarm_msgs::BoundingBoxes &msg);
    void ballon_callback(const swarm_msgs::BoundingBoxes &msg);
    void circle_callback(const swarm_msgs::BoundingBoxes &msg);
    void pipline_callback(const swarm_msgs::BoundingBoxes &msg);
    void stair_callback(const swarm_msgs::BoundingBoxes &msg);

    void ammon_box_callback(const swarm_msgs::BoundingBoxes &msg);
    void water_callback(const swarm_msgs::BoundingBoxes &msg);
    void oil_callback(const swarm_msgs::BoundingBoxes &msg);
    void no_smoking_callback(const swarm_msgs::BoundingBoxes &msg);

    bool checkCollision();

  public:
    EGOReplanFSM(/* args */)
    {
    }
    ~EGOReplanFSM()
    {
    }

    void init(ros::NodeHandle &nh);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

} // namespace ego_planner

#endif